


#include "GeometryUtils.h"




Vec4 GeometryUtils::closestPointOnLineSegment( const Vec4& X, const Vec4& a, const Vec4& b )
{
	Vec4 D = X - a;
	Vec4 T = b - a;
	float l = D*T / (T*T + 1.0e-7f);
	if (l>1.0f) l = 1.0f;
	else if (l<0.0f) l = 0.0f;
	Vec4 Xp = a + l*T;
	
	return Xp;
}


float GeometryUtils::pointToLineSegment( const Vec4& X, const Vec4& a, const Vec4& b, Vec4& Xp )
{
	Xp = GeometryUtils::closestPointOnLineSegment(X, a, b);
	Vec4 d = X - Xp;
	return d.length3();
}


Vec4 GeometryUtils::getClosestPointOnTriangle(const Vec4& P, const Vec4& A, const Vec4& B, const Vec4& C)
{
	float w[3];
	GeometryUtils::getProjectedBarycoords(P, A, B, C, w);
	
	// If the projected point X lies inside the triangle, then the closest point is X
	if (w[0]>=-0.0001f && w[0]<=1.0001f &&
		w[1]>=-0.0001f && w[1]<=1.0001f &&
		w[2]>=-0.0001f && w[2]<=1.0001f )
	{
		Vec4 X = w[0]*A + w[1]*B + w[2]*C;
		return X;
	}
	
	// Otherwise, the closest point is the projection of P onto the closest edge
	Vec4 X01, X12, X20;
	float dX01 = GeometryUtils::pointToLineSegment( P, A, B, X01 );
	float dX12 = GeometryUtils::pointToLineSegment( P, B, C, X12 );
	float dX20 = GeometryUtils::pointToLineSegment( P, C, A, X20 );
	
	if (dX01<dX20)
	{
		if (dX12<dX01) return X12;
		return X01;
	}
	else
	{
		if (dX20<dX12) return X20;
		return X12;
	}
}

float GeometryUtils::vertexTriangleDistance(const Vec4& P, const Vec4& A, const Vec4& B, const Vec4& C)
{
	float w[3];
	GeometryUtils::getProjectedBarycoords(P, A, B, C, w);
	
	// If the projected point X lies inside the triangle, then the closest distance is |X-P|
	if ( w[0]>=-0.001f && w[0]<=1.001f &&
		w[1]>=-0.001f && w[1]<=1.001f &&
		w[2]>=-0.001f && w[2]<=1.001f )
	{
		Vec4 X = w[0]*A + w[1]*B + w[2]*C;
		Vec4 tmp = X-P;
		return tmp.length3();
	}
	
	// Otherwise, the distance is the distance from P to the closest edge
	Vec4 Xp;
	float X01 = GeometryUtils::pointToLineSegment( P, A, B, Xp );
	float X12 = GeometryUtils::pointToLineSegment( P, B, C, Xp );
	float X20 = GeometryUtils::pointToLineSegment( P, C, A, Xp );
	
	return min(X01, min(X12, X20));
}


/*
 Uses the following nice formula of Bridson for the barycoordinates (w0, w1, w2) of the point
 obtained by projecting a point p into the plane of the triangle with vertices (x0, x1, x2):
 
 |x10.x10  x10.x20| |w1| = |x10.(p-x0)|
 |x10.x20  x20.x20| |w2| = |x20.(p-x0)|
 
 where x10 = x1 - x0, etc.
 The projected point is then given by p = w0 x0 + w1 x1 + w2 x2.
 */
void GeometryUtils::getProjectedBarycoords(const Vec4& p, const Vec4& X0, const Vec4& X1, const Vec4& X2, float* w)
{	
	Vec4 X10 = X1 - X0;
	float A = X10 * X10;
	
	Vec4 X20 = X2 - X0;
	float B = X20 * X20;
	
	float C = X10 * X20;
	float det = A*B - C*C;
	
	if (det < 1.0e-7f)
	{
		//printf("Degenerate triangle in _getProjectedBarycoords, aborting\n");
		w[0] = w[1] = w[2] = 1.0e10f;
	}
	float invDet = 1.0f/det;
	
	Vec4 XX0 = p - X0;
	float y0 = X10 * XX0;
	float y1 = X20 * XX0;
	w[1] =  (B*y0 - C*y1)*invDet;
	w[2] = (-C*y0 + A*y1)*invDet;
	w[0] = 1.f - w[1] - w[2];
}


void GeometryUtils::computePlaneEquation(Vec4& normalInOut, const Vec4& pointOnPlane)
{
	normalInOut[3] = -1.f * pointOnPlane * normalInOut;
}



/********************************************************/
/* AABB-triangle overlap test code                      */
/* by Tomas Akenine-Möller                              */
/* Function: int triBoxOverlap(float boxcenter[3],      */
/*          float boxhalfsize[3],float triverts[3][3]); */
/* History:                                             */
/*   2001-03-05: released the code in its first version */
/*   2001-06-18: changed the order of the tests, faster */
/*                                                      */
/* Acknowledgement: Many thanks to Pierre Terdiman for  */
/* suggestions and discussions on how to optimize code. */
/* Thanks to David Hunt for finding a ">="-bug!         */
/********************************************************/

#include <math.h>
#include <stdio.h>

#define X 0
#define Y 1
#define Z 2

#define CROSS(dest,v1,v2) \
dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; 

#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define SUB(dest,v1,v2) \
dest[0]=v1[0]-v2[0]; \
dest[1]=v1[1]-v2[1]; \
dest[2]=v1[2]-v2[2]; 

#define FINDMINMAX(x0,x1,x2,min,max) \
min = max = x0;   \
if(x1<min) min=x1;\
if(x1>max) max=x1;\
if(x2<min) min=x2;\
if(x2>max) max=x2;

static inline int planeBoxOverlap(float normal[3], float vert[3], float maxbox[3])	
{
	int q;
	float vmin[3],vmax[3],v;
	for(q=X;q<=Z;q++)
	{
		v=vert[q];					
		
		if(normal[q]>0.0f)
		{
			vmin[q]=-maxbox[q] - v;	
			vmax[q]= maxbox[q] - v;	
		}
		else
		{
			vmin[q]= maxbox[q] - v;	
			vmax[q]=-maxbox[q] - v;	
		}
	}
	
	if(DOT(normal,vmin)>0.0f) return 0;	 
	if(DOT(normal,vmax)>=0.0f) return 1; 
	
	return 0;
}

/*======================== X-tests ========================*/

#define AXISTEST_X01(a, b, fa, fb)			   \
p0 = a*v0[Y] - b*v0[Z];			       	   \
p2 = a*v2[Y] - b*v2[Z];			       	   \
if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
if(min>rad || max<-rad) return 0;

#define AXISTEST_X2(a, b, fa, fb)			   \
p0 = a*v0[Y] - b*v0[Z];			           \
p1 = a*v1[Y] - b*v1[Z];			       	   \
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
if(min>rad || max<-rad) return 0;

/*======================== Y-tests ========================*/

#define AXISTEST_Y02(a, b, fa, fb)			   \
p0 = -a*v0[X] + b*v0[Z];		      	   \
p2 = -a*v2[X] + b*v2[Z];	       	       	   \
if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
if(min>rad || max<-rad) return 0;

#define AXISTEST_Y1(a, b, fa, fb)			   \
p0 = -a*v0[X] + b*v0[Z];		      	   \
p1 = -a*v1[X] + b*v1[Z];	     	       	   \
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
if(min>rad || max<-rad) return 0;

/*======================== Z-tests ========================*/

#define AXISTEST_Z12(a, b, fa, fb)			   \
p1 = a*v1[X] - b*v1[Y];			           \
p2 = a*v2[X] - b*v2[Y];			       	   \
if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
if(min>rad || max<-rad) return 0;

#define AXISTEST_Z0(a, b, fa, fb)			   \
p0 = a*v0[X] - b*v0[Y];				   \
p1 = a*v1[X] - b*v1[Y];			           \
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
if(min>rad || max<-rad) return 0;


int GeometryUtils::triBoxOverlap(float boxcenter[3],float boxhalfsize[3],float triverts[3][3])
{
	/*    use separating axis theorem to test overlap between triangle and box */
	/*    need to test for overlap in these directions: */
	/*    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
	/*       we do not even need to test these) */
	/*    2) normal of the triangle */
	/*    3) crossproduct(edge from tri, {x,y,z}-directin) */
	/*       this gives 3x3=9 more tests */
	
	float v0[3],v1[3],v2[3];
	
	//   float axis[3];
	float min,max,p0,p1,p2,rad,fex,fey,fez;		
	float normal[3],e0[3],e1[3],e2[3];
	
	/* move everything so that the boxcenter is in (0,0,0) */
	SUB(v0,triverts[0],boxcenter);
	SUB(v1,triverts[1],boxcenter);
	SUB(v2,triverts[2],boxcenter);
	
	/* compute triangle edges */
	SUB(e0,v1,v0);      /* tri edge 0 */
	SUB(e1,v2,v1);      /* tri edge 1 */
	SUB(e2,v0,v2);      /* tri edge 2 *
						 
						 /*  test the 9 tests first (this was faster) */
	fex = fabsf(e0[X]);
	fey = fabsf(e0[Y]);
	fez = fabsf(e0[Z]);
	
	AXISTEST_X01(e0[Z], e0[Y], fez, fey);
	AXISTEST_Y02(e0[Z], e0[X], fez, fex);
	AXISTEST_Z12(e0[Y], e0[X], fey, fex);
	
	fex = fabsf(e1[X]);
	fey = fabsf(e1[Y]);
	fez = fabsf(e1[Z]);
	
	AXISTEST_X01(e1[Z], e1[Y], fez, fey);
	AXISTEST_Y02(e1[Z], e1[X], fez, fex);
	AXISTEST_Z0(e1[Y], e1[X], fey, fex);
	
	fex = fabsf(e2[X]);
	fey = fabsf(e2[Y]);
	fez = fabsf(e2[Z]);
	
	AXISTEST_X2(e2[Z], e2[Y], fez, fey);
	AXISTEST_Y1(e2[Z], e2[X], fez, fex);
	AXISTEST_Z12(e2[Y], e2[X], fey, fex);
	
	/*  first test overlap in the {x,y,z}-directions */
	/*  find min, max of the triangle each direction, and test for overlap in */
	/*  that direction -- this is equivalent to testing a minimal AABB around */
	/*  the triangle against the AABB */
	
	/* test in X-direction */
	FINDMINMAX(v0[X],v1[X],v2[X],min,max);
	if(min>boxhalfsize[X] || max<-boxhalfsize[X]) return 0;
	
	/* test in Y-direction */
	FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max);
	if(min>boxhalfsize[Y] || max<-boxhalfsize[Y]) return 0;
	
	/* test in Z-direction */
	FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max);
	if(min>boxhalfsize[Z] || max<-boxhalfsize[Z]) return 0;
	
	/*  test if the box intersects the plane of the triangle */
	/*  compute plane equation of triangle: normal*x+d=0 */
	CROSS(normal,e0,e1);
	
	if(!planeBoxOverlap(normal,v0,boxhalfsize)) return 0;	
	
	return 1;   /* box and triangle overlaps */
}

#undef X
#undef Y
#undef Z







